#include "MarkerData.h"

using namespace MMM;

MarkerData::MarkerData()
= default;

MarkerData::MarkerData(const std::map<std::string, Eigen::Vector3f> &labeledData, const std::vector<Eigen::Vector3f> &unlabeledData) :
    labeledData(labeledData),
    unlabeledData(unlabeledData)
{
}

bool MarkerData::hasLabeledData() {
    return labeledData.size();
}

bool MarkerData::hasLabel(const std::string &label) {
    return labeledData.find(label) != labeledData.end();
}

Eigen::Vector3f MarkerData::getData(const std::string &label) {
    return labeledData[label];
}

std::map<std::string, Eigen::Vector3f> MarkerData::getLabeledData() {
    return labeledData;
}

std::vector<Eigen::Vector3f> MarkerData::getUnlabeledData() {
    return unlabeledData;
}

bool MarkerData::addMarkerData(const Eigen::Vector3f &data, const std::string &label) {
    if (label.empty()) unlabeledData.push_back(data);
    else {
        if (hasLabel(label)) {
            MMM_ERROR << "Marker Data has label '" << label << "'!" << std::endl;
            return false;
        }
        else labeledData[label] = data;
    }
    return true;
}
